#pragma once
#include "SFML\Graphics.hpp"
#include "animSprite.h"
#include "Pathfinding.h"
#include "Terrain.h"
#include <queue>
#include "config.h"

#include "Unit.h"

namespace game
{
	class TestUnit
	{
		game::UnitState _state;
		animSprite * _sprite;
		sf::RenderWindow * _window;
		float _moveSpeed;
		sf::Vector2f _pos;
		std::queue<sf::Vector2u> _future_move_pos;
	public:
		~TestUnit(){delete _sprite;}
		TestUnit(sf::Vector2u pos, animSprite * spr, sf::RenderWindow * rw)
		{
			_window = rw;
			_state = game::S_UNIT_IDLE;
			_moveSpeed = 5.0f;
			_pos = sf::Vector2f(pos.x,pos.y);
			_sprite = spr;
			LoadAnimation();
		}
		void LoadAnimation()
		{
			_sprite->addFrame(0);
			_sprite->addFrame(0);
			_sprite->addFrame(0);
			_sprite->addFrame(2);
			_sprite->addFrame(2);
			_sprite->addFrame(2);
			_sprite->addFrame(3);
			_sprite->addFrame(3);
			_sprite->addFrame(3);
		}
		void Update()
		{
			if (!_future_move_pos.empty())
			{
				//move towards pos
				sf::Vector2u temp_pos = _future_move_pos.front();
				float dx = (float)temp_pos.x - _pos.x;
				float dy = (float)temp_pos.y - _pos.y;
				float angle = std::atan2(dy,dx);
				_pos.x += _moveSpeed * std::cos(angle);
				_pos.y += _moveSpeed * std::sin(angle);

				float distance_squared = dx*dx + dy * dy;	//distance to target point
				if (_pos == sf::Vector2f(temp_pos.x,temp_pos.y))
				{
					_future_move_pos.pop();
					if(_future_move_pos.empty())
					_state = game::S_UNIT_IDLE;
				}
				if (distance_squared < _moveSpeed * _moveSpeed)
				{
					_pos = sf::Vector2f(temp_pos.x,temp_pos.y);
					_future_move_pos.pop();
					if(_future_move_pos.empty())
					_state = game::S_UNIT_IDLE;
				}
			}

			//_sprite->updatePosition(sf::Vector2u((unsigned int)_pos.x,(unsigned int)_pos.y));
		}
		sf::Vector2u get_current_pos()
		{
			if (_future_move_pos.empty())
			{
				return sf::Vector2u(_pos.x,_pos.y);
			}
			else return _future_move_pos.front();
		}
		sf::Vector2u get_last_future_pos()
		{
			if (!_future_move_pos.empty())
			{
				return _future_move_pos.back();
			}
			else {return sf::Vector2u(_pos.x,_pos.y);}
		}
		bool is_idle()
		{
			return _state == game::S_UNIT_IDLE;
		}
		void Draw()
		{
			//get z offset
			_sprite->updatePosition(sf::Vector2u((unsigned int)_pos.x,(unsigned int)_pos.y));
			_sprite->draw(*_window);
		}
		void MoveToPoint(sf::Vector2u goal)
		{
			//snap to grid
			goal.x -= (goal.x%GRIDSIZE);	
			goal.y -= (goal.y%GRIDSIZE);
			//convert x-cord to tile id
			goal.x /= game::GRIDSIZE;	
			goal.y /= game::GRIDSIZE;
			sf::Vector2u final_point = get_last_future_pos();
			//convert x-cord to tile id
			final_point.x /= game::GRIDSIZE;	
			final_point.y /= game::GRIDSIZE;
			int map[32][32];
			//fill map with air//
			for(int i = 0; i < 32; i++)for (int j = 0; j < 32; j++){map[j][i] = 0;}
			//get direction string//
			std::string path = game::PathFinder::GetPath(final_point.x,final_point.y,goal.x,goal.y,map);
			const int size = path.size();
			for (int i = 0; i < size; i++)
			{
				string o = "";
				if		(path[i] == '0'){o = "Right";}
				else if (path[i] == '1'){ o = "Down";}
				else if (path[i] == '2'){ o = "Left";}
				else if (path[i] == '3'){ o = "Up";}
				if (o!=""){MoveToNextCell(o);}
			}
			
		}
		void MoveToNextCell(std::string key)
		{
			sf::Vector2u cur_pos = get_last_future_pos();
			if		(key == "Up" )	{_future_move_pos.push(sf::Vector2u(cur_pos.x, cur_pos.y -= game::GRIDSIZE));_state = game::S_UNIT_MOVING;}
			else if (key == "Down")	{_future_move_pos.push(sf::Vector2u(cur_pos.x ,cur_pos.y += game::GRIDSIZE));_state = game::S_UNIT_MOVING;}//_gridPos.y+=game::GRIDSIZE;_state = game::S_UNIT_MOVING;}
			else if (key == "Right"){_future_move_pos.push(sf::Vector2u(cur_pos.x += game::GRIDSIZE, cur_pos.y));_state = game::S_UNIT_MOVING;}//_gridPos.x+=game::GRIDSIZE;_state = game::S_UNIT_MOVING;}
			else if (key == "Left")	{_future_move_pos.push(sf::Vector2u(cur_pos.x -= game::GRIDSIZE, cur_pos.y));_state = game::S_UNIT_MOVING;}//_gridPos.x-=game::GRIDSIZE;_state = game::S_UNIT_MOVING;}
		}
	};

}

